#ifndef __EM_PLANNER_LOCATION_HPP__
#define __EM_PLANNER_LOCATION_HPP__

#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <visualization_msgs/Marker.h>
#include <gazebo_msgs/ModelStates.h>
#include <tf/transform_listener.h>



#include "./em_utils.hpp"

class LocationInfo
{
private:
    ros::NodeHandle nh;
    ros::Subscriber model_state_sub;
    Location car_location;
    Location car_pre_location;
    bool FLAG_is_first_run_model_state_callback = true;

private:
    void locationToString();
    void model_state_callback(const gazebo_msgs::ModelStates &msg);

public:
    LocationInfo(/* args */);
    ~LocationInfo();

    void run();
    Location getCurrentLocation() {
        return car_location;
    }
};




#endif